#include <algorithm>
#include <string>
#include <memory>
#include "nav_controller/nav_controller.hpp"
#include "nav2_core/exceptions.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/geometry_utils.hpp"

using nav2_util::declare_parameter_if_not_declared;
using nav2_util::geometry_utils::euclidean_distance;

namespace nav_controller
{

  void CustomController::configure(
      const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
      std::string name, const std::shared_ptr<tf2_ros::Buffer> tf,
      const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
  {
    RCLCPP_INFO(logger_, "configure");
    node_ = parent;

    auto node = node_.lock();

    costmap_ros_ = costmap_ros;
    tf_ = tf;
    plugin_name_ = name;
    logger_ = node->get_logger();
    clock_ = node->get_clock();

    declare_parameter_if_not_declared(
        node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5));
    declare_parameter_if_not_declared(
        node, plugin_name_ + ".lookahead_dist",
        rclcpp::ParameterValue(0.4));
    declare_parameter_if_not_declared(
        node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue(0.5));
    declare_parameter_if_not_declared(
        node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1));

    node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_);
    node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_);
    node->get_parameter(plugin_name_ + ".max_angular_vel", max_angular_vel_);
    node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance_);

    global_pub_ = node->create_publisher<nav_msgs::msg::Path>("received_global_plan", 1);
    laser_util_ = std::make_unique<nav_controller::LaserUtil>(node);
    rviz_marker_ = std::make_unique<rviz_util_pkg::RvizMarker>(node);
  }

  void CustomController::cleanup()
  {
    RCLCPP_INFO(logger_, "cleanup");
    global_pub_.reset();
    laser_util_.reset();
  }

  void CustomController::activate()
  {
    RCLCPP_INFO(logger_, "activate");
    global_pub_->on_activate();
  }

  void CustomController::deactivate()
  {
    RCLCPP_INFO(logger_, "deactivate");
    global_pub_->on_deactivate();
  }

  void CustomController::setSpeedLimit(const double &speed_limit, const bool &percentage)
  {
    (void)speed_limit;
    (void)percentage;
    // RCLCPP_INFO(logger_, "Setting speed limit to %f (percentage: %d)", speed_limit, percentage);
  }
  // 计算线速度、角速度
  geometry_msgs::msg::TwistStamped CustomController::computeVelocityCommands(
      const geometry_msgs::msg::PoseStamped &pose,
      const geometry_msgs::msg::Twist &velocity,
      nav2_core::GoalChecker *goal_checker)
  {
    (void)velocity;
    (void)goal_checker;
    // RCLCPP_INFO(logger_,"global_plan_.header.frame_id:%s",global_plan_.header.frame_id.c_str());//map
    // RCLCPP_INFO(logger_,"pose.header.frame_id:%s",pose.header.frame_id.c_str());//odom
    // RCLCPP_INFO(logger_,"pose:%f,%f",pose.pose.position.x,pose.pose.position.y);
    // RCLCPP_INFO(logger_,"costmap =%s",costmap_ros_->getBaseFrameID().c_str());// base_link
    if (global_plan_.poses.empty())
    {
      throw nav2_core::PlannerException("Received global plan with 0 length");
    }
    // 将机器人当前姿态转换到 机器人坐标系 转为 base_link
    geometry_msgs::msg::PoseStamped pose_frame;
    if (!nav2_util::transformPoseInTargetFrame(pose, pose_frame, *tf_,
                                               costmap_ros_->getBaseFrameID(), transform_tolerance_))
    {
      throw nav2_core::PlannerException("无法将机器人姿态转换为 base_link 坐标系");
    }
    // 将全局路径转换到 机器人坐标系 转为 base_link，并裁剪路径
    nav_msgs::msg::Path global_plan_frame = transformGlobalPlan(pose_frame);

    geometry_msgs::msg::PoseStamped goal_pose = global_plan_frame.poses.back();
    double goal_distance = euclidean_distance(pose_frame.pose, goal_pose.pose);
    // 障碍物距离
    float laser_distance = laser_util_->get_mid_distance();

    // 观察点
    auto lookahead_pose = getLookAheadPose(pose_frame, global_plan_frame);
    // rviz显示观察点
    geometry_msgs::msg::PoseStamped lookahead_pose_global_frame;
    if (nav2_util::transformPoseInTargetFrame(lookahead_pose, lookahead_pose_global_frame, *tf_,
                                              global_plan_.header.frame_id, transform_tolerance_))
    {
      rviz_marker_->clear_ns_points("lookahead");
      rviz_marker_->publish_marker_point(lookahead_pose_global_frame.pose.position.x, lookahead_pose_global_frame.pose.position.y, "lookahead");
    }
    // 发送角速度 正数为逆时针旋转，负数为顺时针旋转
    double angle_diff = calculateAngleDifference(pose_frame, lookahead_pose);

    // Create and publish a TwistStamped message with the desired velocity
    geometry_msgs::msg::TwistStamped cmd_vel;
    cmd_vel.header.frame_id = pose.header.frame_id;
    cmd_vel.header.stamp = clock_->now();

    // 遇到障碍物 三级减速
    double linear_vel;
    if (laser_distance > 1.0)
    {
      linear_vel = desired_linear_vel_;
      if (goal_distance < lookahead_dist_)
      { // 距离目标点 小于 观察点距离
        linear_vel = 0.2 * desired_linear_vel_;
      }
    }
    else if (laser_distance > 0.5)
    {
      linear_vel = 0.5 * desired_linear_vel_;
    }
    else
    {
      linear_vel = 0.2 * desired_linear_vel_;
    }

    // TODO  角度要能按就近旋转计算
    // 根据角度差计算速度，弧度差大于 0.3 则原地旋转，否则直行
    // 0.1 弧度 = 5.729 度
    // 机器人当前的角度和目标点的角度差 大于 固定 弧度，则原地旋转，否则只发送线速度，不旋转

    double angular_vel = (std::fabs(angle_diff) / angle_diff) * max_angular_vel_;

    if (angle_diff > M_PI/10)
    {
      // //到达目标点附近
      // if (goal_distance < lookahead_dist_)
      // {
      //   geometry_msgs::msg::PoseStamped goal_pose = global_plan_frame.poses.back();
      //   double goal_yaw = tf2::getYaw(goal_pose.pose.orientation);
      //   double diff_yaw = goal_yaw - tf2::getYaw(pose_frame.pose.orientation);
      //   angular_vel = (std::fabs(diff_yaw) / diff_yaw) * 0.2;
      // }
      // auto curvature = 2.0 * lookahead_pose.pose.position.y /
                      //  (lookahead_pose.pose.position.x * lookahead_pose.pose.position.x + lookahead_pose.pose.position.y * goal_pose.pose.position.y);
      cmd_vel.twist.linear.x = 0;
      cmd_vel.twist.angular.z = angular_vel ;
    }
    else
    {
      cmd_vel.twist.linear.x = linear_vel;
      cmd_vel.twist.angular.z = 0;
    }
    RCLCPP_INFO(logger_, "发送速度(%.3f,%.3f)", cmd_vel.twist.linear.x, cmd_vel.twist.angular.z);
    return cmd_vel;
  }

  void CustomController::setPlan(const nav_msgs::msg::Path &path)
  {
    global_pub_->publish(path);
    global_plan_ = path;
  }

  geometry_msgs::msg::PoseStamped CustomController::getLookAheadPose(
      const geometry_msgs::msg::PoseStamped &current_pose,
      const nav_msgs::msg::Path &global_plan_frame)
  {
    auto dist_it = std::find_if(global_plan_frame.poses.begin(), global_plan_frame.poses.end(),
                                [&](const geometry_msgs::msg::PoseStamped &pose)
                                { return euclidean_distance(pose.pose, current_pose.pose) > lookahead_dist_; });
    if (dist_it != global_plan_frame.poses.end())
    {
      geometry_msgs::msg::PoseStamped lookahead_pose = *dist_it;
      return lookahead_pose;
    }
    RCLCPP_INFO(logger_, "靠近目标点，返回目标点");
    return global_plan_frame.poses.back();
  }

  double CustomController::calculateAngleDifference(
      const geometry_msgs::msg::PoseStamped &current_pose,
      const geometry_msgs::msg::PoseStamped &target_pose)
  {
    // 计算当前姿态与目标姿态之间的角度差
    // 1. 获取当前位姿的弧度
    double current_robot_yaw = tf2::getYaw(current_pose.pose.orientation);
    // 2.获取目标点朝向
    double target_angle =
        std::atan2(target_pose.pose.position.y - current_pose.pose.position.y,
                   target_pose.pose.position.x - current_pose.pose.position.x);
    //  3.计算角度差，并转换到 -M_PI 到 M_PI 之间
    double angle_diff = target_angle - current_robot_yaw;
    if (angle_diff < -M_PI)
    {
      angle_diff += 2.0 * M_PI;
    }
    else if (angle_diff > M_PI)
    {
      angle_diff -= 2.0 * M_PI;
    }
    RCLCPP_INFO(logger_, "angle_diff: %f", angle_diff);
    return angle_diff;
  }

  nav_msgs::msg::Path CustomController::transformGlobalPlan(geometry_msgs::msg::PoseStamped robot_pose)
  {
    if (global_plan_.poses.empty())
    {
      throw nav2_core::PlannerException("Received plan with 0 length");
    }

    nav2_costmap_2d::Costmap2D *costmap = costmap_ros_->getCostmap();
    // 目标阈值
    double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) *
                            costmap->getResolution() / 2.0;
    // 裁剪点
    auto prune_point = nav2_util::geometry_utils::first_after_integrated_distance(
        global_plan_.poses.begin(), global_plan_.poses.end(), dist_threshold);
    RCLCPP_INFO(logger_, "裁剪点: %ld", std::distance(global_plan_.poses.begin(), prune_point));
    // 起始点 找出离机器人当前位置最近点的迭代器
    auto transformation_begin = std::find_if(
        global_plan_.poses.begin(), prune_point,
        [&](const geometry_msgs::msg::PoseStamped &global_plan_pose)
        {
          return euclidean_distance(robot_pose.pose, global_plan_pose.pose) < dist_threshold;
        });
    // 结束点
    auto transformation_end = std::find_if(
        transformation_begin, global_plan_.poses.end(),
        [&](const geometry_msgs::msg::PoseStamped &pose)
        {
          return euclidean_distance(pose.pose, robot_pose.pose) > dist_threshold;
        });
    // 匿名函数
    auto transformGlobalPoseToLocal = [&](const auto &global_plan_pose)
    {
      // We took a copy of the pose, let's lookup the transform at the current time
      geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose;
      stamped_pose.header.frame_id = global_plan_.header.frame_id;
      stamped_pose.header.stamp = robot_pose.header.stamp;
      stamped_pose.pose = global_plan_pose.pose;
      nav2_util::transformPoseInTargetFrame(
          stamped_pose, transformed_pose,
          *tf_, robot_pose.header.frame_id,
          transform_tolerance_);
      return transformed_pose;
    };
    nav_msgs::msg::Path transformed_plan;
    transformed_plan.header.frame_id = robot_pose.header.frame_id; // base_link
    transformed_plan.header.stamp = robot_pose.header.stamp;
    // 把transformation_begin 到 transformation_end 范围内的元素进行转换
    std::transform(
        transformation_begin, transformation_end,
        std::back_inserter(transformed_plan.poses),
        transformGlobalPoseToLocal);
    return transformed_plan;
  }

} // namespace nav_controller

// Register this controller as a nav2_core plugin
PLUGINLIB_EXPORT_CLASS(nav_controller::CustomController, nav2_core::Controller)